import lejos.nxt.*;
import lejos.nxt.addon.CompassSensor;
import lejos.robotics.navigation.*;

public class Blighbot2 {
	private static final float DIAMETER = 5.6F; // cm
	private static final float TRACKWIDTH = 17.8F; // cm
	private static final Motor LEFTMOTOR = Motor.B;
	private static final Motor RIGHTMOTOR = Motor.C;
	private static final SensorPort COMPASSPORT = SensorPort.S1;
	private static final boolean REVERSE = true;
	private static final boolean CALIBRATE = true;
	
	private static CompassSensor cps;
	private static SimpleNavigator robot;

	public static void main(String[] args) throws Exception {
		if(CALIBRATE)
			CompassCalibrate.calibrate();
		
		cps = new CompassSensor(COMPASSPORT);

		CompassPilot pilot = new CompassPilot(cps, DIAMETER, TRACKWIDTH, LEFTMOTOR, RIGHTMOTOR, REVERSE);
		robot = new SimpleNavigator(pilot);

		Thread compassThread = new CompassThread();
		compassThread.start();

		robot.setTurnSpeed(10); // Turn speed is very low
		
		while(!Button.ENTER.isPressed()){	
		}

		//robot.goTo(200, 0);
		//robot.goTo(100, 100);
		//robot.goTo(100, -50);
		//robot.goTo(0, 0);

		robot.rotateTo(0);
		Thread.sleep(1000);
		robot.rotateTo(90);
		Thread.sleep(1000);
		robot.rotateTo(180);
		Thread.sleep(1000);
		robot.rotateTo(270);
		Thread.sleep(1000);
		robot.rotateTo(0);

		//robot.goTo(20, 0);
		//robot.goTo(0, 0);

		while(!Button.ESCAPE.isPressed()){	
		}
	}

	private static class CompassThread extends Thread {

		public CompassThread() {
			this.setDaemon(true);
		}

		public void run()
		{
			LCD.drawString("Compass:",0,3);
			LCD.drawString("Angle:",0,4);
			while(true) {
				LCD.drawInt((int)cps.getDegrees(), 3, 9, 3);
				LCD.drawInt((int)robot.getAngle(), 3, 9, 4);
				try {
					Thread.sleep(100);
				} catch (Exception e) {
					// Nothing...
				}
			}
		}
	}

};
